#ifndef GAMEENGINE_GEOMETRY_LINE3D_HPP
#define GAMEENGINE_GEOMETRY_LINE3D_HPP

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================
// Implementation of the Line3d class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Line3d<T>::Line3d() {}

//! Constructor from two points
/*!
 @param p1 The first point
 @param p2 The second point
 @note Class T should have a default constructor
 @note Class T should have an assignment operator
 */
template <class T>
Line3d<T>::Line3d(const Point3d<T>& p1, const Point3d<T>& p2) {
	points_[0] = p1;
	points_[1] = p2;
}

//! Copy constructor
/*!
 @param rhs The line to copy
 @note Class T should have a default constructor
 @note Class T should have an assignment operator
 */
template <class T>
Line3d<T>::Line3d(const Line3d& rhs) {
	for ( int k = 0; k < 2; ++k ) {
		points_[k] = rhs.points_[k];
	}
}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Line3d<T>::~Line3d() {}

//! Assignment operator
/*!
 @param rhs The line to copy
 @note Class T should have an assignment operator
 */
template <class T>
Line3d<T>& Line3d<T>::operator=(const Line3d& rhs) {
	if ( &rhs != this ) {
		for ( int k = 0; k < 2; ++k ) {
			points_[k] = rhs.points_[k];
		}
	}
	return *this;
}

//! Accessor to the first point (read-write)
/*!
 @return A reference to the first point
 */
template <class T>
Point3d<T>& Line3d<T>::p1() {
	return points_[0];
}

//! Accessor to the first point (read only)
/*!
 @return A constant reference to the first point
 */
template <class T>
const Point3d<T>& Line3d<T>::p1() const {
	return points_[0];
}

//! Accessor to the second point (read-write)
/*!
 @return A reference to the second point
 */
template <class T>
Point3d<T>& Line3d<T>::p2() {
	return points_[1];
}

//! Accessor to the second point (read only)
/*!
 @return A constant reference to the second point
 */
template <class T>
const Point3d<T>& Line3d<T>::p2() const {
	return points_[1];
}

//! Accessor to the given point (read-write)
/*!
 @return A reference to the given point
 */
template <class T>
Point3d<T>& Line3d<T>::operator[](Line3D::Index index) {
	return points_[(int)index];
}

//! Accessor to the given point (read only)
/*!
 @return A constant reference to the given point
 */
template <class T>
const Point3d<T>& Line3d<T>::operator[](Line3D::Index index) const {
	return points_[(int)index];
}

//! Compute the point location along the line given a parametric coordinate
/*!
 @param coordinate The target parametric coordinate
 @return The point location along the line given a parametric coordinate
 */
template <class T>
Point3d<T> Line3d<T>::point(const T& coordinate) const {
	return Vector3d<T>( points_[0], points_[1] ) * coordinate + points_[0];
}

//! Compute the intersection between the two lines, if and only if the two lines are coplanar
/*!
 @param rhs The line to intersect with
 @param coordinate1 This reference receives the parametric coordinate of the intersection, if
					any. If you want to get the intersection point rather than its parametric
					coordinate, call the point() function on this line
 @param coordinate2 If not nil, the content of this pointer receives the parametric coordinate
					of the intersection along the rhs line, if any intersection. If you want to
					get the intersection point rather than its parametric coordinate, call the
					point() function on this line
 @param tolerance The two lines are unlikely to be exactly coplanar, due to a limited numerical
				  accuracy of type T. Hence, it is unlikely that the two lines share an exact
				  intersection. The tolerance parameter allows an approximate intersection, i.e.,
				  we consider that the two lines intersect if their minimum distance is below the
				  tolerance parameter
 @return The intersection code of the two lines. The returned value can either be NoIntersection if
         the two lines are not coplanar, Parallel if the two lines are coplanar but do not intersect,
		 Intersection if the two lines do intersect, or DegenerateIntersection if any of the lines is
		 degenerate (hence, that intersection does not make sense)
 @note Since the two lines are supposed to have an infinite extension, the ExternIntersection code
       cannot be returned with this type of element
 */
template <class T>
IntersectionType::Type Line3d<T>::intersection(const Line3d& rhs, T& coordinate1, T* coordinate2, const T& tolerance) const {
	// Check that lines are not degenerate
	if ( degenerate() ) return IntersectionType::DegenerateIntersection;
	if ( rhs.degenerate() ) return IntersectionType::DegenerateIntersection;

	// Get denominator
	T denom =  -((-p1().x() + p2().x())*(rhs.p1().x() - rhs.p2().x()) + (-p1().y() + p2().y())*(rhs.p1().y() - rhs.p2().y()) + (-p1().z() + p2().z())*(rhs.p1().z() - rhs.p2().z()))*
			  ((-p1().x() + p2().x())*(rhs.p1().x() - rhs.p2().x()) + (-p1().y() + p2().y())*(rhs.p1().y() - rhs.p2().y()) + (-p1().z() + p2().z())*(rhs.p1().z() - rhs.p2().z()))
			+ ((p1().x() - p2().x())*(p1().x() - p2().x()) + (p1().y() - p2().y())*(p1().y() - p2().y()) + (p1().z() - p2().z())*(p1().z() - p2().z()))*
			  ((rhs.p1().x() - rhs.p2().x())*(rhs.p1().x() - rhs.p2().x()) + (rhs.p1().y() - rhs.p2().y())*(rhs.p1().y() - rhs.p2().y())
			+ (rhs.p1().z() - rhs.p2().z())*(rhs.p1().z() - rhs.p2().z()));

	// Check that the denominator is not null (i.e. that the lines are not parallel
	if ( denom == 0.0f ) {
		return IntersectionType::Parallel;
	}

	coordinate1 = ((-p1().x() + rhs.p1().x())*((rhs.p1().x() - rhs.p2().x())*((p1().x() - p2().x())*(rhs.p1().x() - rhs.p2().x())
		+ (p1().y() - p2().y())*(rhs.p1().y() - rhs.p2().y()) + (p1().z() - p2().z())*(rhs.p1().z() - rhs.p2().z()))
		+ (-p1().x() + p2().x())*((rhs.p1().x() - rhs.p2().x())*(rhs.p1().x() - rhs.p2().x())
		+ (rhs.p1().y() - rhs.p2().y())*(rhs.p1().y() - rhs.p2().y()) + (rhs.p1().z() - rhs.p2().z())*(rhs.p1().z() - rhs.p2().z())))
		+ (-p1().y() + rhs.p1().y())*((rhs.p1().y() - rhs.p2().y())*((p1().x() - p2().x())*(rhs.p1().x() - rhs.p2().x())
		+ (p1().y() - p2().y())*(rhs.p1().y() - rhs.p2().y()) + (p1().z() - p2().z())*(rhs.p1().z() - rhs.p2().z()))
		+ (-p1().y() + p2().y())*((rhs.p1().x() - rhs.p2().x())*(rhs.p1().x() - rhs.p2().x())
		+ (rhs.p1().y() - rhs.p2().y())*(rhs.p1().y() - rhs.p2().y()) + (rhs.p1().z() - rhs.p2().z())*(rhs.p1().z() - rhs.p2().z())))
		+ (-p1().z() + rhs.p1().z())*((-p1().z() + p2().z())*((rhs.p1().x() - rhs.p2().x())*(rhs.p1().x() - rhs.p2().x())
		+ (rhs.p1().y() - rhs.p2().y())*(rhs.p1().y() - rhs.p2().y()) + (rhs.p1().z() - rhs.p2().z())*(rhs.p1().z() - rhs.p2().z()))
		+ ((p1().x() - p2().x())*(rhs.p1().x() - rhs.p2().x()) + (p1().y() - p2().y())*(rhs.p1().y() - rhs.p2().y())
		+ (p1().z() - p2().z())*(rhs.p1().z() - rhs.p2().z()))*(rhs.p1().z() - rhs.p2().z()))) / denom;

	T coord2 = ((-p1().x() + rhs.p1().x())*(rhs.p1().x()*((p1().y() - p2().y())*(p1().y() - p2().y())
		+ (p1().z() - p2().z())*(p1().z() - p2().z())) - rhs.p2().x()*((p1().y() - p2().y())*(p1().y() - p2().y())
		+ (p1().z() - p2().z())*(p1().z() - p2().z())) + (p1().x() - p2().x())*(-(p1().y() - p2().y())*(rhs.p1().y() - rhs.p2().y())
		- (p1().z() - p2().z())*(rhs.p1().z() - rhs.p2().z()))) +
		(-p1().y() + rhs.p1().y())*((rhs.p1().y() - rhs.p2().y())*((p1().x() - p2().x())*(p1().x() - p2().x())
		+ (p1().y() - p2().y())*(p1().y() - p2().y()) + (p1().z() - p2().z())*(p1().z() - p2().z()))
		+ (-p1().y() + p2().y())*((p1().x() - p2().x())*(rhs.p1().x() - rhs.p2().x()) + (p1().y() - p2().y())*(rhs.p1().y() - rhs.p2().y())
		+ (p1().z() - p2().z())*(rhs.p1().z() - rhs.p2().z())))
		+ (-p1().z() + rhs.p1().z())*((-p1().z() + p2().z())*((p1().x() - p2().x())*(rhs.p1().x() - rhs.p2().x())
		+ (p1().y() - p2().y())*(rhs.p1().y() - rhs.p2().y()) + (p1().z() - p2().z())*(rhs.p1().z() - rhs.p2().z()))
		+ ((p1().x() - p2().x())*(p1().x() - p2().x()) + (p1().y() - p2().y())*(p1().y() - p2().y())
		+ (p1().z() - p2().z())*(p1().z() - p2().z()))*(rhs.p1().z() - rhs.p2().z()))) / denom;
	if ( coordinate2 ) *coordinate2 = coord2;

	// Compute point locations
	Point3d<T> point1 = point(coordinate1);
	Point3d<T> point2 = rhs.point(coord2);

	// Check the distance between the points
	if ( point1.get_distance(point2) <= tolerance ) return IntersectionType::Intersection;
	return IntersectionType::NoIntersection;
}

//! Compute the point location along the line that is the closest to given point
/*!
 @param rhs The point to project on the line
 @return The parametric coordinate of the closest point location along the line
 */
template <class T>
T Line3d<T>::closest_point(const Point3d<T>& rhs) const {
	T denom = p1().get_square_distance(p2());
	T num =   p1().x()*p1().x() + p2().x()*rhs.x() - p1().x()*(p2().x() + rhs.x())
			+ p1().y()*p1().y() + p2().y()*rhs.y() - p1().y()*(p2().y() + rhs.y())
			+ (p1().z() - p2().z())*(p1().z() - rhs.z());
	return num / denom;
}

//! Compute the point location along the line that is the closest to given line
/*!
 @param rhs The line for which to compute the closest point
 @param type If not nil, this value receives secondary information about the closest
             point. For example, if the two lines are parallel, there is an infinity
			 of "closest" points. This parameter also gives access to the information
			 whether the closest point is shared by the two lines (i.e., that the two
			 lines intersect) or not. The default tolerance value of 0.000001 is used
			 to test for intersection
 @return The parametric coordinate of the closest point location along the line
 @note If the two lines are parallel, returned value is always 0
 */
template <class T>
T Line3d<T>::closest_point(const Line3d& rhs, IntersectionType::Type* type) const {
	// Compute the closest point
	T coord;
	IntersectionType::Type type_tmp = intersection(rhs, coord);
	if ( type ) *type = type_tmp;

	// Return result
	if ( type_tmp == IntersectionType::Parallel ) return 0;
	return coord;
}

//! Check whether the line is degenerate
/*!
 @return True if the line is degenerate, false otherwise
 @note The line is degenerate if its first and second points
       have exactly the same location
 */
template <class T>
bool Line3d<T>::degenerate() const {
	return ( p1().get_square_distance(p2()) <= 0 );
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
